#include "SensorDataKinectInput.h"

SensorDataKinectInput::SensorDataKinectInput (const std::string &name,
		char* configFile) : SensorDataInput (name)
{
	XnStatus rc;
	xn::EnumerationErrors errors;
	XnFieldOfView fov;

	rc = _context.InitFromXmlFile (configFile, &errors);
	if (rc == XN_STATUS_NO_NODE_PRESENT) {
		XnChar strError[1024];
		errors.ToString (strError, 1024);
		OutputDebugStringA (strError);
		OutputDebugStringA ("\n");
		throw (rc);
	} else if (rc != XN_STATUS_OK) {
		char strError[1024];
		sprintf(strError, "Open failed: %s\n", xnGetStatusString(rc));
		OutputDebugStringA (strError);
		throw (rc);
	}
	_context.SetGlobalMirror (false);
	rc = _context.FindExistingNode (XN_NODE_TYPE_IMAGE, _image);
	if (rc == XN_STATUS_OK) {
		_image.GetMetaData (_imageMD);
		__rgbMetaData = cvCreateImage (cvSize (_imageMD.FullXRes (),
				_imageMD.FullYRes ()), 8, 3);
		cvZero (__rgbMetaData);
		__sensorParameter.rgbPixelWidth = sizeof (XnRGB24Pixel);
		__sensorParameter.frameWidth = _imageMD.XRes ();
		__sensorParameter.frameHeight = _imageMD.YRes ();
	}

	rc = _context.FindExistingNode (XN_NODE_TYPE_DEPTH, _depth);
	if (rc == XN_STATUS_OK) {
		if (__rgbMetaData)
			_depth.GetAlternativeViewPointCap ().SetViewPoint (_image);
		_depth.GetMetaData (_depthMD);
		_depth.GetFieldOfView (fov);
		__depthMetaData = cvCreateImage (
				cvSize (__sensorParameter.frameWidth,
				__sensorParameter.frameHeight), IPL_DEPTH_16U, 1);
		cvZero (__depthMetaData);
		__sensorParameter.depthPixelWidth = sizeof (XnDepthPixel);
		__sensorParameter.frameWidth = _depthMD.XRes ();
		__sensorParameter.frameHeight = _depthMD.YRes ();
		__sensorParameter.frameDepth = _depthMD.ZRes ();
		__sensorParameter.frameHFOV = fov.fHFOV;
		__sensorParameter.frameVFOV = fov.fVFOV;
		__sensorParameter.frameVFOVAngle = glm::tan (fov.fHFOV / 2);
		__sensorParameter.frameHFOVAngle = glm::tan (fov.fVFOV / 2);

	}
	__sensorParameter.frameSize =  __sensorParameter.frameWidth *
			__sensorParameter.frameHeight;
}

SensorDataKinectInput::~SensorDataKinectInput () {}

bool SensorDataKinectInput::grabFrame ()
{
	XnStatus rc = XN_STATUS_OK;

	// Read a new frame
	rc = _context.WaitAnyUpdateAll ();
	if (rc != XN_STATUS_OK) {
		printf("Read failed: %s\n", xnGetStatusString(rc));
		return false;
	}
	int frameWidth = __sensorParameter.frameWidth;
	int frameHeight = __sensorParameter.frameHeight;
	if (__rgbMetaData) {
		_image.GetMetaData (_imageMD);
		for (int v = 0; v < frameHeight; ++v)
			for (int u = 0; u < frameWidth; ++u) {
				int pixelOffset = (v * frameWidth + u) * __sensorParameter.rgbPixelWidth;
				__rgbMetaData->imageData[pixelOffset] = _imageMD.Data ()[pixelOffset + 2];
				__rgbMetaData->imageData[pixelOffset + 1] = _imageMD.Data ()[pixelOffset + 1];
				__rgbMetaData->imageData[pixelOffset + 2] = _imageMD.Data ()[pixelOffset];
			}
	}

	if (__depthMetaData) {
		_depth.GetMetaData(_depthMD);
		int depthFrameBytes = __sensorParameter.frameSize *
				__sensorParameter.depthPixelWidth;
		memcpy_s (__depthMetaData->imageData, depthFrameBytes, _depthMD.Data (),
				depthFrameBytes);
		for (int v = 0; v < frameHeight; ++v)
			for (int u = 0; u < frameWidth; ++u)
				if (CV_IMAGE_ELEM (__depthMetaData, XnDepthPixel, v, u) == 0)
					CV_IMAGE_ELEM (__depthMetaData, XnDepthPixel, v, u) =
							__sensorParameter.frameDepth;
	}
	return true;
}